% $Id: StopsAnalyzer.m 4 2008-11-12 23:31:05Z Jie.Bao $ 

classdef StopsAnalyzer
    %STOPSANALYZER finds stops in a set of X,Y,Z coordinates
    %   Determiness the duration of these stops

    properties
        X;                       %x coordinate of stop
        Y;                       %y coordinate of stop
        Z;                       %z coordiante of stop
        T;
        MinAngle;                %Smallest angle stop sweeps through
        MaxAngle;                %Max angle stop sweeps through
        TotalSweep               %Sum of total angle swept
        Duration;                %duration of stop
        StopTime;
        
                             
    end
    
    properties (Constant=true)
        %constants
        SizeMultiplier = 2;      %multiplier to make stop dots bigger. 
                                 %   easier to see then
        MIN_VELOCITY = .02;         %all velocities under this are considered stops
        VELOCITY_INTERVAL = 1;      %how many points L and R to average for a velocity
                                    %with 1, the velocity at each point
                                    %will be the average of the instant
                                    %velocities before it, and after it
                                    %with 2 it will average the velocities
                                    %between point and 2 points left, and 2
                                    %points right
    end

    methods
        %accessors and getters
        function obj = set.SizeMultiplier(obj, size)
            obj.SizeMultiplier = size;
        end
        
        function obj = set.MIN_VELOCITY(obj, min)
            obj.MIN_VELOCITY = min;
        end
        

        
        %Main constructor. Takes x,y,z,t coordinates and finds all stops
        function obj = StopsAnalyzer(t,x,y,z)
            if nargin == 3
                z = zeros(length(x), 1);  % 2-D coordinates
            end
            V = obj.Velocity(x,y,z,t);
            INT = obj.VELOCITY_INTERVAL;
            t_smooth = t(1+INT:length(t)-INT);
            len = length(V);
            counter = 0;                  %tracks length of a stop
            x_index = 0;                  %tracks x coord of a stop
            y_index = 0;                  %tracks y coord of a stop
            z_index = 0;                  %tracks z coord of a stop
            min_angle =0;
            max_angle =0;
            angle_sum = 0;
            start = 0;                    %sentinel to determine if stopped before or not
            begin = 1;                    %index in the stops array
            stops =[];
            for j=1:len
                %begin timing a stop 
                if V(j) < obj.MIN_VELOCITY && start == 0
                    counter = counter + 1;
                    stop_begin = j;
                    x_index = x(j);
                    y_index = y(j);
                    z_index = z(j);
                    start = 1;


                %continue timing a stop
                elseif V(j) < obj.MIN_VELOCITY && start == 1
                    counter = counter + 1;


                %stop timing a stop and add it to the list
                elseif V(j) > obj.MIN_VELOCITY && start == 1
                    start = 0;                      %reset sentinel
                    stop_end = j - 1;
                    stops(begin, 1) = x_index;      %#ok<AGROW> %x coord of stop
                    stops(begin, 2) = y_index;      %#ok<AGROW> %y coord of stop
                    stops(begin, 3) = z_index;      %#ok<AGROW> %z coord of stop
                    stops(begin, 4) = counter;
                    stops(begin, 5) = t_smooth(stop_end) - ...
                        t_smooth(stop_begin);      %#ok<AGROW> %time of stop
                    begin = begin+1;                %counter in stops array
                    counter = 0;                    %reset time counter
                end                  
            end 
             
            if ( ~ isempty(stops))
                obj.X = stops(:,1);
                obj.Y = stops(:,2);
                obj.Z = stops(:,3);
                obj.Duration = stops(:,4);
                obj.StopTime = stops(:,5);
            end
            
        end

        %main function to plot the PStops
        %hold_on is used to save what was previously in the plot
        %    this is used to overlay things, like stps, floorplans
        %it multiplies the durations by a constant to get bigger dots so
        %   everything is actually visible.
        function Plot(obj, hold_on)
           %do not overwrite previous plot, add to it
           if hold_on
               hold on
           end

           scatter(obj.X, obj.Y, obj.Duration*obj.SizeMultiplier, 'filled'); 

        end
        
        % get the total number of stops
        function nStops = TotalNumber(obj)
            nStops = length(obj.Duration);
        end
        
        % get the total duration of stops
        function stops_duration = TotalDuration(obj)
            stops_duration = sum(obj.StopTime);
        end
        

        %helper function
        %calculates velocity based on the xyz coordinates
        %averages instantaneous velocities across nearby sectors to smooth
        %things out a bit
        function v = Velocity(obj,x,y,z,t)
            v = zeros(length(x)-2*obj.VELOCITY_INTERVAL, 1);
            INT = obj.VELOCITY_INTERVAL;

            for i=1+INT:length(x)-INT
                vel_back    = distance(x(i), x(i-INT), ...
                                       y(i), y(i-INT), ...
                                       z(i), z(i-INT)) / (t(i) - t(i-INT));
                vel_forward = distance(x(i), x(i+INT), ...
                                       y(i), y(i-INT), ...
                                       z(i), z(i+INT)) / (t(i+INT) - t(i));
                v(i-INT) = (vel_back + vel_forward) / 2;
            end
        end
    end
end
